#include "mpu6050.h"
#include "Fusion.h"
#include "drv_mpu6050.h"

#define MPU6050_DEVICE_NAME  "mpu 6050"
#define SAMPLE_PERIOD (0.01f)
static rt_thread_t mpu6050_tid = RT_NULL;
static rt_timer_t mpu6050_timer;
static struct mpu6xxx_device *mpu6050_dev;
//static struct mpu6xxx_3axes mpu6050_accel, mpu6050_gyro;
static FusionVector gyroscope,accelerometer;
static FusionAhrs ahrs;
static FusionEuler euler;
static int pitch,roll,yaw;



//定时恢复mpu6050数据采集线程
static void mpu6050_timeout(void *p)
{
    //恢复mpu6050数据采集线程并立马切换线程
    rt_thread_resume(mpu6050_tid);
    rt_schedule();

}

//mpu6050原始数据获得
static void mpu6050_entry(void *p)
{
    while(1)
    {
        //调度器上锁 防止被中断
//        rt_enter_critical();
        //获取数据
        //rt_kprintf("s:%d\n",rt_tick_get());
//        mpu6xxx_get_gyro(mpu6050_dev, &mpu6050_gyro);
//        mpu6xxx_get_accel(mpu6050_dev, &mpu6050_accel);
        //调度器解锁
//        rt_exit_critical();

        //计算真实值
//        gyroscope.array[0] = (float)(mpu6050_gyro.x) / 16.384;
//        gyroscope.array[1] = (float)(mpu6050_gyro.y) / 16.384;
//        gyroscope.array[2] = (float)(mpu6050_gyro.z) / 16.384;
//        accelerometer.array[0] = (float)(mpu6050_accel.x) / 2048;
//        accelerometer.array[1] = (float)(mpu6050_accel.y) / 2048;
//        accelerometer.array[2] = (float)(mpu6050_accel.z) / 2048;
        //解算姿态
//        FusionAhrsUpdateNoMagnetometer(&ahrs,gyroscope,accelerometer,SAMPLE_PERIOD);
//        euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
//        pitch = euler.angle.pitch*100;
//        roll = euler.angle.roll*100;
//        yaw = euler.angle.yaw*100;
//        //rt_kprintf("e:%d\n",rt_tick_get());
//        rt_kprintf("%d,%d,%d\r\n", pitch,roll,yaw);

        //挂起当前线程并立马进行上下文切换
        rt_thread_suspend(mpu6050_tid);
        rt_schedule();
    }
}

//外部调用初始化mpu6050
int mpu6050_thread_init(void)
{
    //初始化mpu6050
    Mpu6050_Init();
    //mpu6050_dev = mpu6xxx_init(MPU6050_DEVICE_NAME, RT_NULL);
//    if (mpu6050_dev == RT_NULL)
//    {
//        rt_kprintf("mpu6050 init failed\n");
//        return -1;
//    }
//    rt_kprintf("mpu6050 init succeed\n");

    //创建读取数据线程
//    mpu6050_tid = rt_thread_create("mpu 6050", mpu6050_entry, RT_NULL, 1024, 1, 20);
//    if (mpu6050_tid != RT_NULL) {
//            rt_kprintf("mpu6050_tid init\r\n");
//            rt_thread_startup(mpu6050_tid);
//        }
//        else{
//            rt_kprintf("mpu6050_tid = RT_NULL\r\n");
//            return RT_ERROR;
//        }
//
//    //创建定时器周期处理数据
//    mpu6050_timer =  rt_timer_create("mpu6050 timer", mpu6050_timeout,
//                                    RT_NULL, SAMPLE_PERIOD*1000,
//                                    RT_TIMER_FLAG_PERIODIC);
//    if (mpu6050_timer != RT_NULL) {
//        rt_timer_start(mpu6050_timer);
//        rt_kprintf("mpu6050 timer start\r\n");
//    }else{
//        rt_kprintf("mpu6050 timer = RT_NULL\r\n");
//        return RT_ERROR;
//    }

    //初始化算法数据
    FusionAhrsInitialise(&ahrs);
    return 0;
}
